#include "VehicleCurrentState.h"
#include <Physics/Vehicle/hkpVehicleInstance.h>
#include <Physics/Dynamics/Entity/hkpRigidBody.h>
#include <math.h>
#include <crtdbg.h>


VehicleCurrentState::State::State(void)
{
	HK_VehiclePosition.setZero4();
	HK_VehicleQuaternion.setAxisAngle(hkVector4(0.0f, 1.0f, 0.0f), 0.0f);
	CurrentSpeed = 0.0f;

	HK_ChassisForwardAxisWS.setZero4();
	HK_ChassisRightAxisWS.setZero4();
	HK_ChassisUpAxisWS.setZero4();
	HK_VelocityWS.setZero4();
	HK_VelocityDirectionWS.setZero4();
	HK_VelocityOS.setZero4();
	HK_VelocityDirectionOS.setZero4();

	HK_2D_ChassisForwardAxisWS.set(0.0f, 0.0f);
	HK_2D_ChassisRightAxisWS.set(0.0f, 0.0f);
	HK_2D_VelocityWS.set(0.0f, 0.0f);
};


VehicleCurrentState::VehicleCurrentState(void)
: m_vehicleStateMap()
{

}


VehicleCurrentState::~VehicleCurrentState(void)
{
	m_vehicleStateMap.clear();
}


void VehicleCurrentState::check_valid(const hkpVehicleInstance* pVehicleInstance) const
{
	_ASSERT(pVehicleInstance);
}


void VehicleCurrentState::add(const hkpVehicleInstance* pVehicleInstance)
{
	check_valid(pVehicleInstance);
	if(!m_vehicleStateMap.count(pVehicleInstance)) m_vehicleStateMap[pVehicleInstance] = State();
}


void VehicleCurrentState::refresh_state(const hkpVehicleInstance* pVehicleInstance)
{
	check_valid(pVehicleInstance);
	if (m_vehicleStateMap.count(pVehicleInstance))
	{
		State& vehicleState = m_vehicleStateMap[pVehicleInstance];
		hkpRigidBody* pRigidBody = pVehicleInstance->getChassis();
		vehicleState.HK_VehiclePosition = pRigidBody->getPosition();
		vehicleState.HK_VehicleQuaternion = pRigidBody->getRotation();
		vehicleState.CurrentSpeed = pVehicleInstance->calcKMPH();
		vehicleState.HK_ChassisForwardAxisWS.setRotatedDir(vehicleState.HK_VehicleQuaternion, hkVector4(1.0f, 0.0f, 0.0f));
		vehicleState.HK_ChassisRightAxisWS.setRotatedDir(vehicleState.HK_VehicleQuaternion, hkVector4(0.0f, 0.0f, 1.0f));
		vehicleState.HK_ChassisUpAxisWS.setRotatedDir(vehicleState.HK_VehicleQuaternion, hkVector4(0.0f, 1.0f, 0.0f));
		vehicleState.HK_VelocityWS = pRigidBody->getLinearVelocity();
		vehicleState.HK_VelocityDirectionWS = vehicleState.HK_VelocityWS;
		if (vehicleState.HK_VelocityDirectionWS.length3() != 0.0f) vehicleState.HK_VelocityDirectionWS.normalize3();
		vehicleState.HK_VelocityOS.setRotatedInverseDir(vehicleState.HK_VehicleQuaternion, vehicleState.HK_VelocityWS);
		vehicleState.HK_VelocityDirectionOS = vehicleState.HK_VelocityOS;
		if (vehicleState.HK_VelocityDirectionOS.length3() != 0.0f)vehicleState.HK_VelocityDirectionOS.normalize3();
	}
}


void VehicleCurrentState::remove(const hkpVehicleInstance* pVehicleInstance)
{
	check_valid(pVehicleInstance);
	if (m_vehicleStateMap.count(pVehicleInstance)) m_vehicleStateMap.erase(pVehicleInstance);
}


const VehicleCurrentState::State* VehicleCurrentState::get_state(const hkpVehicleInstance* pVehicleInstance)
{
	check_valid(pVehicleInstance);
	if (m_vehicleStateMap.count(pVehicleInstance)) return &(m_vehicleStateMap[pVehicleInstance]);
	return NULL;
}